#include "DataProcessor.h"

#include <QThread>
#include <QDebug>
#include <cstring>
#include <iomanip>
#include <iostream>

const u8 CRC16_TABLE[SIZE_MEDIUM]
    = {50,  124, 137, 0,   237, 217, 104, 119, 0,   0,   0,   89,  0,   0,   0,   0,   0,   0,   0,   0,   214, 159, 220, 168, 24,  23,
       170, 144, 67,  115, 39,  246, 185, 104, 237, 244, 222, 212, 9,   254, 230, 28,  28,  132, 221, 232, 11,  153, 41,  39,  0,   0,
       0,   0,   15,  3,   0,   230, 0,   0,   0,   153, 183, 51,  82,  118, 148, 21,  0,   243, 124, 0,   0,   38,  20,  158, 152, 143,
       0,   0,   0,   106, 49,  22,  143, 140, 5,   150, 0,   231, 183, 63,  54,  0,   0,   0,   0,   0,   0,   0,   175, 102, 158, 208,
       56,  93,  138, 108, 32,  185, 84,  34,  0,   124, 237, 4,   76,  128, 56,  116, 134, 237, 203, 250, 87,  203, 220, 25,  226, 46,
       29,  223, 85,  6,   229, 203, 1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   154, 49,  0,   134, 219, 208, 188, 84,  22,
       19,  21,  134, 0,   78,  68,  189, 127, 154, 21,  21,  144, 1,   234, 73,  181, 22,  83,  167, 138, 234, 240, 47,  189, 52,  0,
       0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   204, 49,  170, 44,  83,  46,  0};

DataProcessor::DataProcessor(MulticastSender& sender, QObject* parent)
    : QObject(parent)
    , running(true)          // 初始化运行标志为 true
    , sender_(sender)        // 引用 MulticastSender 对象，用于发送数据
{
    // // 启动数据处理线程
    // QThread* processorThread = new QThread();
    // this->moveToThread(processorThread);

    // // 连接线程启动信号到 processingLoop 槽
    // connect(processorThread, &QThread::started, this, &DataProcessor::processingLoop);

    // // 确保线程对象在完成后被删除
    // connect(this, &DataProcessor::destroyed, processorThread, &QThread::quit);
    // connect(processorThread, &QThread::finished, processorThread, &QObject::deleteLater);

    // // 启动线程
    // processorThread->start();
}

DataProcessor::~DataProcessor()
{
    {
        QMutexLocker locker(&queueMutex); // 确保线程安全
        running = false; // 停止数据处理线程
    }
    cv.wakeAll(); // 唤醒线程以便退出
    // 线程会在 processingLoop 中自行退出并清理
}

void DataProcessor::onDataReceived(const std::string& data)
{
    processRecvData(data);
   /* QMutexLocker locker(&queueMutex);   // 确保队列操作线程安全
    dataQueue.enqueue(data);            // 将数据添加到队列
    cv.wakeOne();      */                 // 唤醒处理线程
}

// void DataProcessor::sendDataToMulticast(const std::string& message)
// {
//     sender_.sendToMulticastGroup(message);
// }

void DataProcessor::processingLoop()
{
    while (true)
    {
        std::string data;
        {
            QMutexLocker locker(&queueMutex);
            if (dataQueue.isEmpty())
            {
                cv.wait(&queueMutex); // 等待数据到来或退出信号
                if (!running && dataQueue.isEmpty())
                {
                    break; // 如果退出标志已设置且队列为空，退出线程
                }
            }

            if (!dataQueue.isEmpty())
            {
                data = dataQueue.dequeue(); // 从队列中取出数据
            }
        }

        if (!data.empty())
        {
            // 处理数据
            processRecvData(data);

            // 模拟处理时间
            QThread::msleep(100);
        }

        // 检查是否需要退出
        {
            QMutexLocker locker(&queueMutex);
            if (!running && dataQueue.isEmpty())
            {
                break;
            }
        }
    }
}

/**
 * @brief 处理收到的数据
 * @param data 接收到的数据
 */
void DataProcessor::processRecvData(const std::string& data)
{
    TMavlinkString mavStr;
    memcpy(&mavStr, data.data(), data.size());

    u16 recv_crc = static_cast<u16>(mavStr.Data[mavStr.LEN] | (mavStr.Data[mavStr.LEN + 1] << 8));
    u16 calc_crc = mavlinkCrc16Accumulate(reinterpret_cast<const u8*>(data.data()), static_cast<s32>(data.size() - MAVLINK_CRC16_SIZE));
    if (recv_crc != calc_crc)
    {
        printHex(data);
        std::cout << "ground recv message crc16: " << recv_crc << " calculate crc16: " << calc_crc << std::endl;
        return;
    }

    // 根据消息 ID 处理不同类型的消息
    switch (mavStr.MsgID)
    {
    case MAVLINK_MSG_ID_ATTITUDE:
    {
        TMavlinkAttitude msgInfo;
        memcpy(&msgInfo, mavStr.Data, mavStr.LEN);

        std::cout << "boot time: " << msgInfo.time << std::endl;
        std::cout << "roll angle: " << msgInfo.roll << std::endl;
        std::cout << "pitch angle: " << msgInfo.pitch << std::endl;
        std::cout << "yaw angle: " << msgInfo.yaw << std::endl;
        std::cout << std::endl;
    }
    break;

    case MAVLINK_MSG_ID_POSITION:
    {
        TMavlinkPosition msgInfo;
        memcpy(&msgInfo, mavStr.Data, mavStr.LEN);

        std::cout << "boot time: " << msgInfo.time << std::endl;
        std::cout << "latitude: " << msgInfo.lat << std::endl;
        std::cout << "longitude: " << msgInfo.lon << std::endl;
        std::cout << "altitude: " << msgInfo.alt << std::endl;
        std::cout << "relative: " << msgInfo.relative_alt << std::endl;
        std::cout << "x-velocity: " << msgInfo.vx << std::endl;
        std::cout << "y-velocity: " << msgInfo.vy << std::endl;
        std::cout << "z-velocity: " << msgInfo.vz << std::endl;
        std::cout << "distance: " << msgInfo.distance2home << std::endl;
        std::cout << std::endl;
    }
    break;

    default:
    {
        std::cout << "##### unknown MSG ID: " << static_cast<u32>(mavStr.MsgID) << std::endl;
    }
    break;
    }

    // 根据需要发送命令数据包
    // 例如，收到特定消息后发送命令
    // processSendData();
}

/**
 * @brief 打印十六进制信息
 * @param data 要打印的数据
 */
void DataProcessor::printHex(const std::string& data)
{
    for (size_t i = 0; i < data.size(); ++i)
    {
        std::cout << std::setw(2) << std::setfill('0') // 设置每个数字宽度为2，并用 '0' 填充
                  << std::hex << (0xFF & static_cast<u8>(data[i])) << " ";
    }
    std::cout << std::endl;
}

/**
 * @brief MAVLink CRC16 单字节累积校验
 * @param data 单个字节数据
 * @param crc_data 当前 CRC 值
 * @return 更新后的 CRC 值
 */
u16 DataProcessor::mavlinkCrc16AccumulateSingle(const u8 data, u16 crc_data)
{
    u8 ch;

    ch       = static_cast<u8>(data ^ static_cast<u8>(crc_data & 0x00ff));
    ch       = static_cast<u8>(ch ^ (ch << 4));
    crc_data = static_cast<u16>((crc_data >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4));

    return crc_data;
}

/**
 * @brief MAVLink CRC16 缓冲区累积校验
 * @param data_buffer 数据缓冲区
 * @param buffer_length 缓冲区长度
 * @return 计算后的 CRC16 值
 */
u16 DataProcessor::mavlinkCrc16Accumulate(const u8* data_buffer, s32 buffer_length)
{
    u16 crc = 0xf00f;

    for (s32 i = 1; i < buffer_length; i++)
    {
        crc = mavlinkCrc16AccumulateSingle(data_buffer[i], crc);
    }
    crc = mavlinkCrc16AccumulateSingle(CRC16_TABLE[data_buffer[5]], crc);

    return crc;
}

/**
 * @brief MAVLink 格式信息打包
 * @param data 数据内容
 * @param len 数据长度
 * @param msgID 消息 ID
 * @param dataOut 输出的数据包
 * @return 打包后的数据长度
 */
s32 DataProcessor::mavlinkMessagePacking(const void* data, const u8& len, u8 msgID, TMavlinkString* dataOut)
{
    static u8 idx = 0;

    memset(dataOut, 0, sizeof(TMavlinkString));
    memcpy(&dataOut->Data[0], data, len);

    dataOut->STX_0 = MAVLINK_MSG_HEAD_0;
    dataOut->STX_1 = MAVLINK_MSG_HEAD_1;
    dataOut->CLASS = MAVLINK_MSG_CLASS;
    dataOut->LEN   = len;
    dataOut->SEQ   = idx++;
    dataOut->MsgID = msgID;

    u16 crc_16 = mavlinkCrc16Accumulate(reinterpret_cast<u8*>(dataOut), len + MAVLINK_HEADER_SIZE);
    memcpy(&dataOut->Data[len], &crc_16, sizeof(u16));

    return (len + MAVLINK_HEADER_CRC16_SIZE);
}

/**
 * @brief 发送命令数据包
 */
void DataProcessor::processSendData()
{
    u8             msgID;
    std::string    message;
    TMavlinkString replayMsg;

    msgID = MAVLINK_MSG_ID_COMMAND;
    switch (msgID)
    {
    case MAVLINK_MSG_ID_COMMAND:
    {
        TMavlinkControl msgInfo;
        memset(&msgInfo, 0x00, sizeof(TMavlinkControl));

        msgInfo.command = 20; // 返航
        msgInfo.Param_1 = 1;
        msgInfo.Param_2 = 0;

        s32 replayLen = mavlinkMessagePacking(&msgInfo, sizeof(TMavlinkControl), msgID, &replayMsg);

        message = std::string(reinterpret_cast<s8*>(&replayMsg), static_cast<u32>(replayLen));
    }
    break;

    default:
    {
        std::cout << "##### unknown MSG ID: " << static_cast<u32>(msgID) << std::endl;
        return;
    }
    }

    // 发出信号，发送消息到组播组
    emit sendDataToMulticast(message);
}
